安全平面功能介绍

修订日期	修订版本	修订内容	修订人
2024.2.1	v0.1	初始化文档	邓誉鑫

1 功能描述

本文档介绍的安全平面和安全立方体功能主要应用于速度模式下的机器人运动，通过使用speedL来实现。目前该功能支持末端点的减速，不支持肘部点的减速。

安全平面

用于确保机械臂在接近该平面时能够在安全范围内减速并停止运动，以避免发生碰撞或越界，安全平面通过utlDefinePlane 接口来定义，通过传入N+1个点（无限平面时 N >= 3，定义有限平面时 N = 3，另一个点为规定平面内侧方向的点）来计算平面参数（a，b，c，d），然后通过mdlSetLinkCollisionGeometryModel接口将安全平面（Plane）设置为环境中的几何体碰撞模型。

无限制的安全平面： 可以传入至少4个点来确定唯一的无限平面：若传入四个点，需要保证前三个点不重合以及不共线，以前三个点建立一个平面；若传入大于四个点，则通过拟合方式来确定一个平面。最后一个点确定平面的内外侧，表示平面的朝向，需要保证不与前三个点确定的平面共面。根据轨迹点的运动方向确定减速策略：运动方向接近安全平面时，能够在机械臂的减速能力范围内开始减速，并在安全平面附近处停止，运动方向远离安全平面时不受限制
有限制的安全平面：只能传入四个点来确定唯一的有限平面，目前支持矩形形状。在设置平面碰撞模型前，需要使用三点标定平面中心处的坐标系origin，确定平面衍生的X和Y方向，这里标定使用的三点与建立平面的前三点相同，并且需要设定平面的延展范围extent（均需要大于零，默认小于零为无限平面）通过 mdlSetLinkCollisionGeometryModel接口将标定的origin坐标系传入到设定平面中。在有限制的安全平面中，机械臂运动时会根据轨迹点的位置和当前速度方向判断是否可以穿过该安全平面，如果轨迹点的运动方向在安全平面边界内，机械臂将在安全平面处减速并停止，超过安全平面边界则不受限制

无限制平面无需设置origin，因为没有中心坐标系的概念，而有限平面则通过三点标定origin来确定一个有边界平面: 其中第一点作为origin的原点，第二点给定origin的x轴方向，第三点给定origin的y轴方向（根据两次右手定则，第一次根据第一点与第二点确定过渡轴，第二次根据过渡轴与x轴确定y轴）

安全立方体

安全立方体用于限制机械臂运动的立体区域。通过mdlSetLinkCollisionGeometryModel接口将安全平面（Box）设置为环境中的几何体碰撞模型。它可以分为两种主要类型：可达安全立方体和环境中的障碍物。

可达安全立方体： 可达安全立方体是机械臂能够安全到达的三维区域。通过定义立方体的边界范围，确保机械臂在运动时不会超出这个范围，从而避免与其它物体碰撞。

此类安全立方体是在六个方向上定义了六个安全平面，在机械臂靠近立方体任意一个面时均会触发减速策略

环境中的障碍物： 这种类型的安全立方体用于检测环境中的障碍物。机械臂在运动时，系统会实时监测末端与障碍物的距离，并根据设定的逻辑，在靠近障碍物并可能发生碰撞的情况下减速并停止运动。

与有限制安全平面一致，若运动方向超出立方体的边界范围，则不会受到限制

Box对应的origin表示Box中心点的坐标变换，若指定origin为零，则默认在基座标系原点处建立Box，否则以设置的origin为中心点建立Box

2 接口介绍

/**
 * @brief 使用N + 1个点定义一个平面: 定义无限平面时 N >= 3，定义有限平面时 N = 3
 * @param points: 三维坐标点构成的矩阵, points = {P1, P2, ..., Pn}, points.size()必须大于等于3
 * @param point_direction: 一个三维坐标点，用于规定平面内侧的方向, point_direction = {x0, y0, z0}
 * @param plane: 输出的平面
 * @return ret < 0: 无法定义平面
 */
    ARAL_API_BASIC(1.0) int utlDefinePlane(const std::vector<std::vector<double>>& points, const std::vector<double>& point_direction, geometric_shapes::Plane& plane)const = 0;

/**
 * @brief 设置机械臂连杆对应的碰撞几何体模型
 * @param link_name: 连杆名称，通过　mdlGetLinkName　获得
 * @param shapes: 几何模型
 * @param origins: 几何模型参考坐标系在连杆坐标系的位姿，若为有限平面，将平面中心处坐标系传入；若为无限平面，默认设置为单位矩阵；若为立方体，将立方体中心处变换矩阵传入
 * @return　< 0, 表示设置失败
*/
    ARAL_API_COMMON(1.0) int mdlSetLinkCollisionGeometryModel(const std::string& link_name, const std::vector<geometric_shapes::ShapePtr>& shapes, const std::vector<RLPose>& origins)const = 0;


//! 几何体类型: 平面
//! 在机械臂的实际应用场景中，可以使用三点(或者多于三点来标定一个平面), 另外可以根据额外的点来确定平面的方向
//! 如果是有限平面(支持矩形), 可以使用上述的三点同时标定一个坐标系，其中矩形的延展方向分别是X和Y方向
class Plane : public Shape
{
public:
    /** \brief 平面表达式为： Ax + By + Cz + D = 0, 如果P(x0, y0, z0) 为平面内侧的一点, 则Ax0 + By0 + Cz0 + D > 0 */
    double a_, b_, c_, d_;

    Vector2d  extent_{-1, -1};  // 平面的延展范围, 如果都大于0, 则表示为有限平面
};

//! 几何体类型: 长方体(盒子,与XYZ轴相对齐)
class Box : public Shape
{
public:
    Vector3d size_;             //盒子的x, y, z维度(轴对齐)
};

3 开发例程

    auto type = getTestType();
    std::vector<double> maxV = {2, 2, 2, 3.14, 3.14, 3.14};
    std::vector<double> maxA = {1.2, 1.2, 1.2, 1.2, 1.2, 1.2};
    std::vector<double> maxJ = {9.6, 9.6, 9.6, 9.6, 9.6, 9.6};
    interface::MoveProperty move_property = generateMoveProperty(interface::MoveMode::VELOCITY, 1, maxV, maxA, maxJ);
    interface::RLJntArray q0;         // 本测试需要的输入数据
    interface::RLPose pose;
    std::string link_name = "world";  // 通过指定link_name为"world"可设置环境中几何物体
    geometric_shapes::Plane* plane;
    interface::RLPose origin;
    geometric_shapes::ShapePtr shape;
    if(type.first == 1)
    {
        q0 = {0.6330928073150806, 0.7542270825026934, -1.3758930406000738, -2.2534022983832918, -0.2406789674792817, 1.2892682061984584};
        move_property.tarV = {0, 0.05, 0, 0, 0, 0};
    }
    else if(type.first == 2)         // 测试安全平面(无限)
    {
        q0 = {-1.42833, 0.73985, 1.71810, -0.56636, 1.56207, -1.49575};
        move_property.tarV = {0, -0.5, 0, 0, 0, 0};
        move_property.move_duration = 80;
        plane     = new geometric_shapes::Plane(0, 1, 0, 0.5);   // 通过指定参数创建一个平面
        origin    = {0, 0, 0, 0, 0, 0};                       // 无限平面无需设置中心点坐标系
        shape     = static_cast<geometric_shapes::ShapePtr>(plane);
    }
    else if(type.first == 3)   // 测试安全平面(有限，可以穿过), 平面通过 utlDefinePlane 接口计算
    {
        q0 = {-1.61818, -0.08128, 1.60762, 0.14547, 1.56717, -1.68554};
        points = { {-0.5, 0, 0},{-0.5, 0.5, 0},{-0.5, 0.5, 0.5} };
        point_direction = {0,0,0};
        move_property.tarV = {-0.5, 0, 0, 0, 0, 0};
        move_property.move_duration = 1;
        plane = new geometric_shapes::Plane(0, 0, 0, 0);
        int ret = robot->utlDefinePlane(points, point_direction, *plane);
        CHECK(ret >= 0);
        plane->setExtent({0.2, 0.2});
        origin    = {-0.5, 0, 0, 1.5708,0,-1.5708};
        shape     = static_cast<geometric_shapes::ShapePtr>(plane);
    }
    else if(type.first == 4)  // 测试安全平面(有限，不可以穿过), 平面通过 utlDefinePlane 接口计算
    {
        q0 = {-1.61818, -0.08128, 1.60762, 0.14547, 1.56717, -1.68554};
        points = { {-0.5, 0, 0},{-0.5, 0.5, 0},{-0.5, 0.5, 0.5} };
        point_direction = {0,0,0};
        move_property.tarV = {-0.5, 0, 0, 0, 0, 0};
        move_property.move_duration = 1;
        plane = new geometric_shapes::Plane(0, 0, 0, 0);
        int ret = robot->utlDefinePlane(points, point_direction, *plane);
        CHECK(ret >= 0);
        plane     = new geometric_shapes::Plane(1, 0, 0, 0.5);
        plane->setExtent({1, 1});
        origin    = {-0.5, 0, 0, 1.5708,0,-1.5708};
        shape     = static_cast<geometric_shapes::ShapePtr>(plane);
    }
    else if(type.first == 5)         // 测试安全立方体(环境障碍物)
    {
        q0 = {-1.61818, -0.08128, 1.60762, 0.14547, 1.56717, -1.68554};
        move_property.tarV = {0, 0, -0.5, 0, 0, 0};
        move_property.move_duration = 1;
        box     = new geometric_shapes::Box(0.6, 0.6, 0.6);
        origin    = {0, -0.5, 0, 0, 0, 0};
        shape     = static_cast<geometric_shapes::ShapePtr>(box);
    }
    else if(type.first == 6)         // 测试安全立方体(可达空间)
    {
        q0 = {-1.61818, -0.08128, 1.60762, 0.14547, 1.56717, -1.68554};
        move_property.tarV = {-0.5, 0, 0, 0, 0, 0};
        move_property.move_duration = 1;
        box     = new geometric_shapes::Box(1, 1, 1);
        origin    = {0, 0, 0, 0, 0, 0};
        shape     = static_cast<geometric_shapes::ShapePtr>(box);
    }
    Setup(getRobotType(type.second));
    initiatePlanner(robot, q0, tcp);
    int ret = robot->mdlSetLinkCollisionGeometryModel(link_name, {shape}, {origin}); // 设置机械臂连杆对应的碰撞几何体模型，这里仅设置环境中的碰撞几何体模型
    CHECK(ret >= 0);
    move_property.dynamic_adjust_motion_constraints = true; // 开启动态调速
    ret = robot->tpAddVelocityLine(1, interface::DescribeSpace::CARTESIAN, move_property);
    CHECK(ret >= 0);

    std::vector<interface::TrajectoryPoint> plan_points;
    std::vector<interface::RLJntArray> command_pos, command_vel, command_acc;
    std::vector<double> t;
    ret = generateTrajectoryPoints(robot, q0, 0.005, plan_points, command_pos, command_vel, command_acc, t);
    CHECK(ret >= 0);

4 注意事项

在使用有限安全平面时，请确保标定平面的四个点以及延展范围有效，以便机械臂能够正常穿过或减速运动。
动态调速功能可通过设置 move_property.dynamic_adjust_motion_constraints = true 来开启，确保机械臂在运动中能够根据安全平面实时调整速度，以确保安全性。

3.9.安全平面功能介绍